#include "pch.h"
#include "PointCloudHandler.h"
using namespace AutomatedSurfaceReconstruction;

CGALPointSetType CPointCloudHandler::process(const std::vector<Point2D>& vPixelSet, CGALPointSetType& vPointCloud, const std::pair<Point2D,Point2D>& vAABB)
{
	std::vector<Point2D> BoundaryPoints4XY;
	std::pair<Point2D, Point2D> AABB = __getAABB(vPointCloud);
	for (auto OriginPoint : vPixelSet)
		BoundaryPoints4XY.emplace_back(__map2PointCloud(__map2UV(OriginPoint, vAABB), AABB));

	CGALPointSetType RemovePoints = __removePointsInsideBoundary(BoundaryPoints4XY, vPointCloud);

	pcl::PointCloud<pcl::PointXY>::Ptr Cloud(new pcl::PointCloud<pcl::PointXY>);
	for (auto Iter = RemovePoints.begin(); Iter != RemovePoints.end(); Iter++)
		Cloud->emplace_back(RemovePoints.point(*Iter).x(), RemovePoints.point(*Iter).y());
	pcl::KdTreeFLANN<pcl::PointXY> KdTree;
	KdTree.setInputCloud(Cloud);

	std::vector<Point3D> BoundaryPoints4XYZ;
	for (auto Point4XY : BoundaryPoints4XY)
		BoundaryPoints4XYZ.emplace_back(Point3D(Point4XY.x(), Point4XY.y(), __computeHeightFromNeighbors(Point4XY, KdTree, RemovePoints)));

	std::vector<Kernel::Segment_3> SegmentSet(__jointPoint2Segment(BoundaryPoints4XYZ));
	CGALPointSetType AddPointSet(__addBoundaryPoints(SegmentSet));
	__clearBoundaryAround(vPointCloud, SegmentSet);
	vPointCloud.join(AddPointSet);

	return AddPointSet;
}

void CPointCloudHandler::simplifyPointCloud(CGALPointSetType& vPoints)
{
	double spacing = CGAL::compute_average_spacing<CGAL::Sequential_tag>(vPoints, 6);
	// Simplify using a grid of size 2 * average spacing
	typename CGALPointSetType::iterator gsim_it = CGAL::grid_simplify_point_set(vPoints, 2. * spacing);
	vPoints.remove(gsim_it, vPoints.end());
	std::cout << vPoints.number_of_removed_points()
		<< " point(s) removed after simplification." << std::endl;
	vPoints.collect_garbage();
}

CGALPointSetType CPointCloudHandler::__removePointsInsideBoundary(const std::vector<Point2D>& vBoundaryPointSequence, CGALPointSetType& vPoints)
{
	CGALPointSetType RetainPoints, RemovePoints;
	for (auto Point : vPoints)
	{
		if (CGAL::bounded_side_2(vBoundaryPointSequence.cbegin(), vBoundaryPointSequence.cend(), Point2D(vPoints.point(Point).x(), vPoints.point(Point).y()), Kernel()) == CGAL::ON_UNBOUNDED_SIDE)
		{
			RetainPoints.insert(vPoints.point(Point));
		}
		else
			RemovePoints.insert(vPoints.point(Point));
	}

	vPoints = RetainPoints;
	return RemovePoints;
}

std::vector<Kernel::Segment_3> CPointCloudHandler::__jointPoint2Segment(const std::vector<Point3D>& vBoundaryPoints)
{
	std::vector<Kernel::Segment_3> Segments;
	for (std::size_t i = 0; i < vBoundaryPoints.size(); i++)
	{
		Segments.emplace_back(Kernel::Segment_3(vBoundaryPoints[i], vBoundaryPoints[(i + 1) % vBoundaryPoints.size()]));
	}

	return Segments;
}

CGALPointSetType CPointCloudHandler::__addBoundaryPoints(const std::vector<Kernel::Segment_3>& vSegmentSet)
{
	auto computeMinRoadWidth = [&]()
	{
		float MinRoadWidth = 10;
		for (auto SegmentIter = vSegmentSet.cbegin(); SegmentIter != vSegmentSet.cend(); SegmentIter++)
		{
			for (auto Iter = vSegmentSet.cbegin(); Iter != vSegmentSet.cend(); Iter++)
			{
				float SegmentDistance = CGAL::sqrt(CGAL::squared_distance(*(SegmentIter), *(Iter)));
				MinRoadWidth = (SegmentDistance < MinRoadWidth&& SegmentDistance >10) ? SegmentDistance : MinRoadWidth;
			}
		}
		return MinRoadWidth;
	};

	float MinRoadWidth = computeMinRoadWidth();
	const float PointStep = 0.75 * MinRoadWidth;

	auto getAddPointNum4RoadSegment = [&](const Kernel::Segment_3& vSegment)
	{
		return static_cast<std::size_t>(CGAL::sqrt(vSegment.squared_length()) / PointStep) + 1;
	};

	CGALPointSetType GeneratedPointSet;
	for (auto SegmentIter = vSegmentSet.cbegin(); SegmentIter != vSegmentSet.cend(); SegmentIter++)
	{
		std::size_t AddPointNum = getAddPointNum4RoadSegment(*SegmentIter);
		float SegmentLength = CGAL::sqrt(SegmentIter->squared_length());
		Point3D UnitDirectionVector(SegmentIter->direction().dx() / SegmentLength,
			SegmentIter->direction().dy() / SegmentLength,
			SegmentIter->direction().dz() / SegmentLength);
		for (std::size_t i = 0; i < AddPointNum; i++)
		{
			Point3D GeneratedPoint(SegmentIter->source().x() + i * PointStep * UnitDirectionVector.x(),
				SegmentIter->source().y() + i * PointStep * UnitDirectionVector.y(),
				SegmentIter->source().z() + i * PointStep * UnitDirectionVector.z());
			GeneratedPointSet.insert(GeneratedPoint);
		}
	}

	return GeneratedPointSet;
}

Point2D CPointCloudHandler::__map2UV(const Point2D& vPoint, std::pair<Point2D, Point2D> vAABB)
{
	return Point2D((vPoint.x()-vAABB.first.x()) / (vAABB.second.x() - vAABB.first.x()), 
		(vPoint.y()-vAABB.first.y()) / (vAABB.second.y() - vAABB.first.y()));
}

Point2D CPointCloudHandler::__map2PointCloud(const Point2D& vPoint, const std::pair<Point2D, Point2D>& vAABB)
{
	return Point2D(vAABB.first.x() + vPoint.x() * (vAABB.second.x() - vAABB.first.x()), vAABB.first.y() + vPoint.y() * (vAABB.second.y() - vAABB.first.y()));
}

std::pair<Point2D, Point2D> CPointCloudHandler::__getAABB(const CGALPointSetType& vPoints)
{
	std::pair<float, float> MinPointAABB(FLT_MAX, FLT_MAX), MaxPointAABB(FLT_MIN, FLT_MIN);
	for (auto Iter = vPoints.begin(); Iter != vPoints.end(); Iter++)
	{
		if (vPoints.point(*Iter).x() < MinPointAABB.first)MinPointAABB.first = vPoints.point(*Iter).x();
		if (vPoints.point(*Iter).y() < MinPointAABB.second)MinPointAABB.second = vPoints.point(*Iter).y();
		if (vPoints.point(*Iter).x() > MaxPointAABB.first)MaxPointAABB.first = vPoints.point(*Iter).x();
		if (vPoints.point(*Iter).y() > MaxPointAABB.second)MaxPointAABB.second = vPoints.point(*Iter).y();
	}

	return std::make_pair<Point2D, Point2D>(Point2D(MinPointAABB.first, MinPointAABB.second), Point2D(MaxPointAABB.first, MaxPointAABB.second));
}

float CPointCloudHandler::__computeHeightFromNeighbors(const Point2D& vPoint, pcl::KdTreeFLANN<pcl::PointXY>& vKdTree, const CGALPointSetType& vPoints, const std::size_t vNeighborNum)
{
	std::vector<int> PointIndexKNNSearch(vNeighborNum);
	std::vector<float> PointDistanceKNNSearch(vNeighborNum);
	vKdTree.nearestKSearch(pcl::PointXY(vPoint.x(), vPoint.y()), vNeighborNum, PointIndexKNNSearch, PointDistanceKNNSearch);

	float SumZ = 0;
	for (auto Index : PointIndexKNNSearch)
	{
		SumZ += vPoints.point(Index).z();
	}
	return SumZ / PointIndexKNNSearch.size();
}

void CPointCloudHandler::__clearBoundaryAround(CGALPointSetType& vPointCloud, const std::vector<Kernel::Segment_3>& vSegmentSet, const float vClearRadius)
{
	CGALPointSetType RetainPoints;
	const float ClearSquaredRadius = vClearRadius * vClearRadius;
	std::vector<CGALPointSetType::Index> RemovePointIndexSet;
	for (auto PointIter = vPointCloud.begin(); PointIter != vPointCloud.end(); PointIter++)
	{
		bool IsRetain = true;
		for (auto SegmentIter = vSegmentSet.cbegin(); SegmentIter != vSegmentSet.cend(); SegmentIter++)
		{
			if (CGAL::squared_distance(vPointCloud.point(*PointIter), *SegmentIter) < ClearSquaredRadius)
			{
				IsRetain = false;
				break;
			}
		}
		if (IsRetain)RetainPoints.insert(vPointCloud.point(*PointIter));
	}

	std::swap(vPointCloud, RetainPoints);
}
